//#include "RGBDCamera.h"
//#include "./libbow/trunk/src/Descriptor.cpp"
//#include "./libbow/trunk/src/Codebook.cpp"
//#include "./libbow/trunk/src/CodebookFile.cpp"
//#include "BoWModel.cpp"
//#include "Map.h"
//#include "TestMatchers.cpp"
//#include <stdlib.h>
#include <string>
#include "RGBDCamera.h"
#include "RGBDCamera_FB.h"

using namespace std;
 

int main(){
	string kth_path1= "/media/storage/RGBD/RGBD_Dataset/daily_20110816_085255/dfki-sb/data/rgbd/";
	string kth_path2= "/media/storage/RGBD/RGBD_Dataset/daily_20110816_085255/vienna/data/rgbd/";
	string kth_path3= "/media/storage/RGBD/RGBD_Dataset/daily_20110816_085255/ljubljana/data/data/office1-rgbd/";
	string kth_path4= "/media/storage/RGBD/RGBD_Dataset/daily_20110816_085255/ljubljana/data/data/office2and3-rgbd/";

	string fb_path1 = "/home/johang/slam_data/rgbd_dataset_freiburg2_large_with_loop/";
	string fb_path2 = "/home/johang/slam_data/rgbd_dataset_freiburg1_360/";
	string fb_path3 = "/home/johang/slam_data/rgbd_dataset_freiburg1_desk/";
	string fb_path4 = "/home/johang/slam_data/rgbd_dataset_freiburg1_floor/";
	
	string store = "./store/";

	RGBDCamera * camera1 = new RGBDCamera;
	camera1->setup(fb_path3);
	camera1->print();
	camera1->store(store+"dummy_cam");
	camera1->load(store+"dummy_cam");

	RGBDCamera * camera2 = new RGBDCamera_FB;
	camera2->setup(fb_path3);
	for(int i = 0; i < camera2->nr_frames; i++)
	{
		printf("loading framenode %i\n",i);
		RGBDFrame * frame = camera2->getFrame(i);
	}
	//camera2->print();
	//camera2->store(store+"fb1_cam");
	//camera2->load(store+"fb1_cam");
/*
	string codebookFileName = store+"codebook100";
	//RGBDCamera ** cameras = new RGBDCamera*[1];
	//cameras[0] = new RGBDCamera(path2,store);
	//cameras[1] = new RGBDCamera(path3,store);
	//cameras[2] = new RGBDCamera(path4,store);
	//BoWModel * bow = new BoWModel(cameras,1,codebookFileName,15,100);
	
	string codebookFileName_pos = codebookFileName+"_pos";
	string codebookFileName_neg = codebookFileName+"_neg";
	printf("______________________________________________________________________________________________________________________________________________________________________________________\n");
	printf("store:%s\n",codebookFileName_pos.c_str());
	printf("store:%s\n",codebookFileName_neg.c_str());
	
	CodebookFile codebookFile_pos(codebookFileName_pos);
	CodebookFile codebookFile_neg(codebookFileName_neg);

	if (!codebookFile_pos.openForReading())
	{
		cerr<<"Error: Cannot open the codebook file \""<<codebookFileName_pos<<"\"."<<endl;
		return 1;
	}
	
	if (!codebookFile_neg.openForReading())
	{
		cerr<<"Error: Cannot open the codebook file \""<<codebookFileName_neg<<"\"."<<endl;
		return 1;
	}

	Codebook * codebook_pos = codebookFile_pos.readCodebook();
	Codebook * codebook_neg = codebookFile_neg.readCodebook();
	codebookFile_pos.close();
	codebookFile_neg.close();
	
	int nr_clusters_pos = codebook_pos->getCodebookSize();
	int nr_clusters_neg = codebook_neg->getCodebookSize();
	printf("read codebook_pos: %i\n",nr_clusters_pos);
	printf("read codebook_neg: %i\n",nr_clusters_neg);
	printf("______________________________________________________________________________________________________________________________________________________________________________________\n");

	//RGBDCamera * my_kinect = new RGBDCamera(path1,store);
//	my_kinect->setCodebook(codebook_pos,codebook_neg);
	//exit(0);
	
	//TestMatchers * tester = new TestMatchers(my_kinect);
	//tester->test();
	//delete tester;
	//exit(0);
	
	//Map * map = new Map();
	//map->optimizeTransformation(my_kinect);
	//map->slam_once(my_kinect);
	//map->load("./store/loltest");
	//exit(0);
	//delete my_kinect;
	//delete map;
*/
	return 0;

}
